﻿#include <stdio.h>
#include <cstring>

bool read_int(const char* buf, int length, int& pos, int& v)
{
	if (buf && pos >= 0 && pos + 4 <= length)
	{
		v = ((unsigned char)buf[pos]) | 
			(((unsigned char)buf[pos + 1]) << 8)| 
			(((unsigned char)buf[pos + 2]) << 16)|
			(((unsigned char)buf[pos + 3]) << 24);
		pos += 4;
		return true;
	}
	return false;
}

bool read_unsigned_int(const char* buf, int length, int& pos, unsigned int& v)
{
	if (buf && pos >= 0 && pos + 4 <= length)
	{
		v = ((unsigned char)buf[pos]) |
			(((unsigned char)buf[pos + 1]) << 8) |
			(((unsigned char)buf[pos + 2]) << 16) |
			(((unsigned char)buf[pos + 3]) << 24);
		pos += 4;
		return true;
	}
	return false;
}

bool read_bytes(const char* buf, int length,int& pos,unsigned char* dst, int readLen)
{
	if (buf && dst && pos >= 0 && pos + readLen <= length)
	{
		memcpy(dst, buf + pos, readLen);
		pos += readLen;
		return true;
	}
	return false;
}

bool write_ushort(char* buf, int length, int& pos,unsigned short v)
{
	if (pos < 0 || pos + 2 > length)
		return false;

	buf[pos] = (unsigned char)(v & 0xFF);
	buf[pos + 1] = (unsigned char)((v >> 8) & 0xFF);

	pos += 2;

	return true;
}

bool write_short(char* buf, int length, int& pos,short v)
{
	if (pos < 0 || pos + 2 > length)
		return false;

	buf[pos] = (unsigned char)(v & 0xFF);
	buf[pos + 1] = (unsigned char)((v >> 8) & 0xFF);

	pos += 2;

	return true;
}

bool write_int(char* buf,int length,int& pos,int v)
{
	if (pos < 0 || pos + 4 > length)
		return false;

	buf[pos] = (unsigned char)(v & 0xFF);
	buf[pos + 1] = (unsigned char)((v >> 8) & 0xFF);
	buf[pos + 2] = (unsigned char)((v >> 16) & 0xFF);
	buf[pos + 3] = (unsigned char)((v >> 24) & 0xFF);

	pos += 4;

	return true;
}

bool read_float(const char* buf, int length, int& pos, float& vf)
{
	int v = 0;
	if (!read_int(buf, length, pos, v))
		return false;
	return v * 0.001f;
}

bool write_float(char* buf, int length, int& pos, float vf)
{
	if (pos < 0 || pos + 4 > length)
		return false;

	int v = (int)(vf * 1000);
	buf[pos] = (unsigned char)(v & 0xFF);
	buf[pos + 1] = (unsigned char)((v >> 8) & 0xFF);
	buf[pos + 2] = (unsigned char)((v >> 16) & 0xFF);
	buf[pos + 3] = (unsigned char)((v >> 24) & 0xFF);

	pos += 4;

	return true;
}

bool write_v3(char* buf,int length,int& pos,const float* v)
{
	if (!write_int(buf, length, pos, (int)(v[0] * 1000)))
	{
		return false;
	}
	if (!write_int(buf, length, pos, (int)(v[1] * 1000)))
	{
		return false;
	}
	if (!write_int(buf, length, pos, (int)(v[2] * 1000)))
	{
		return false;
	}
	return true;
}

bool read_v3(const char* buf, int length, int& pos, float* v3buf)
{
	if (!v3buf)
		return false;

	int x, y, z;
	if (!read_int(buf, length, pos, x) || !read_int(buf, length, pos, y) || !read_int(buf, length, pos, z))
	{
		return false;
	}

	v3buf[0] = x * 0.001f;
	v3buf[1] = y * 0.001f;
	v3buf[2] = z * 0.001f;

	return true;
}